package com.fitzpatrickit.robotics.hexaped;

import lejos.nxt.NXTRegulatedMotor;
import lejos.robotics.navigation.Move;
import lejos.robotics.navigation.MoveController;
import lejos.robotics.navigation.MoveListener;

public class HexapedMoveController implements MoveController {
	
	private final NXTRegulatedMotor centerMotor;
	private final NXTRegulatedMotor rightMotor;
	private final NXTRegulatedMotor leftMotor;
	
	private boolean stopped = true;
	
	private Thread movementThread = null;
	
	// enumeration of all possible (valid) states that the robot could be in
	// the enumeration is coded as follows:
	// First letter indicates which set of legs is currently weighted
	// and can be one of: R, L or E indicating right, left or equally weighted
	// Second letter indicates the position of the left set of legs and
	// can be one of F, C or B indicating forward, center or back
	// Third letter indicates the position of the right set of legs and
	// can be one of F, C or B also
	protected enum State {
		E_C_C,
		L_C_C,
		R_C_C,
		L_B_F,
		R_F_B,
		L_F_B,
		R_B_F,
		L_F_F,
		L_B_B,
		R_F_F,
		R_B_B
	}
	
	protected State state = State.E_C_C;
	
	protected static final int LEFT_LEG_RANGE_DEGS = 15 * 2;
	
	protected static final int RIGHT_LEG_RANGE_DEGS = 15 * 2;
	
	protected static final int ROCKER_RANGE_DEGS = 300;
	
	protected static final int HORIZONTAL_SPEED = 100;
	
	protected static final int ROCKER_SPEED = 200;
	
	public HexapedMoveController(NXTRegulatedMotor centerMotor,
			NXTRegulatedMotor rightMotor,
			NXTRegulatedMotor leftMotor) {
		
		this.centerMotor = centerMotor;
		this.rightMotor = rightMotor;
		this.leftMotor = leftMotor;
		
		centerMotor.setSpeed(ROCKER_SPEED);
		rightMotor.setSpeed(HORIZONTAL_SPEED);
		leftMotor.setSpeed(HORIZONTAL_SPEED);
	}

	@Override
	public Move getMovement() {
		// TODO Auto-generated method stub
		return null;
	}

	@Override
	public void addMoveListener(MoveListener listener) {
		// TODO Auto-generated method stub
		
	}
	
	public void forward(int steps) {
		this.moveForward(steps);
	}

	@Override
	public void forward() {
		this.moveForward(0);
	}
	
	public void backward(int steps) {
		this.moveBackward(steps);
	}

	@Override
	public void backward() {
		this.moveBackward(0);
	}

	@Override
	public void stop() {
		this.stopped = true;
		
		if (null != movementThread) {
			try {
				movementThread.join();
			} catch (InterruptedException e) {
				// ignore
			}
			
			movementThread = null;
		}
		
		this.returnToStartState();
	}

	@Override
	public boolean isMoving() {
		return !this.stopped;
	}

	@Override
	public void travel(double distance) {
		// TODO Auto-generated method stub
		
	}

	@Override
	public void travel(double distance, boolean immediateReturn) {
		// TODO Auto-generated method stub
		
	}

	@Override
	public void setTravelSpeed(double speed) {
		// TODO Auto-generated method stub
		
	}

	@Override
	public double getTravelSpeed() {
		// TODO Auto-generated method stub
		return 0;
	}

	@Override
	public double getMaxTravelSpeed() {
		// TODO Auto-generated method stub
		return 0;
	}
	
	public void reset() {
		this.stop();
	}
	
	public void turnLeft() {
		turnLeft(0);
	}
	
	public void turnLeft(final int steps) {
		this.reset();
		
		this.stopped = false;
		
		Runnable turnLeftRunnable = new Runnable() {
			
			public void run() {
				
				int stepsTurned = 0;
				
				try {
					while (!stopped && ( stepsTurned < steps || 0 == steps) ) {
						switch (state) {
							case E_C_C:
								rockRight(ROCKER_RANGE_DEGS / 2, false);
								state = State.R_C_C;
								break;
							case R_C_C:
								leftLegsBackward(LEFT_LEG_RANGE_DEGS / 2, true);
								rightLegsBackward(RIGHT_LEG_RANGE_DEGS / 2, false);
								state = State.R_B_B;
								break;
							case R_B_B:
								rockLeft(ROCKER_RANGE_DEGS, false);
								state = State.L_B_B;
								break;
							case L_B_B:
								leftLegsForward(RIGHT_LEG_RANGE_DEGS, true);
								rightLegsForward(LEFT_LEG_RANGE_DEGS, false);
								state = State.L_F_F;
								break;
							case L_F_F:
								rockRight(ROCKER_RANGE_DEGS, false);
								state = State.R_F_F;
								break;
							case R_F_F:
								leftLegsBackward(LEFT_LEG_RANGE_DEGS, true);
								rightLegsBackward(RIGHT_LEG_RANGE_DEGS, false);
								state = State.R_B_B;
								break;
							default:
								break;
						}
						
						stepsTurned++;
					}
					
				} finally {
					stopped = true;
				}
			}
		};
		
		movementThread = new Thread(turnLeftRunnable);
		movementThread.start();
	}
	
	public void turnRight() {
		turnRight(0);
	}
	
	public void turnRight(final int steps) {
		this.reset();
		
		this.stopped = false;
		
		Runnable turnRightRunnable = new Runnable() {
			
			public void run() {
				
				int stepsTurned = 0;
				
				try {
					while (!stopped && ( stepsTurned < steps || 0 == steps) ) {
						switch (state) {
							case E_C_C:
								rockLeft(ROCKER_RANGE_DEGS / 2, false);
								state = State.L_C_C;
								break;
							case L_C_C:
								leftLegsBackward(LEFT_LEG_RANGE_DEGS / 2, true);
								rightLegsBackward(RIGHT_LEG_RANGE_DEGS / 2, false);
								state = State.L_B_B;
								break;
							case L_B_B:
								rockRight(ROCKER_RANGE_DEGS, false);
								state = State.R_B_B;
								break;
							case R_B_B:
								leftLegsForward(RIGHT_LEG_RANGE_DEGS, true);
								rightLegsForward(LEFT_LEG_RANGE_DEGS, false);
								state = State.R_F_F;
								break;
							case R_F_F:
								rockLeft(ROCKER_RANGE_DEGS, false);
								state = State.L_F_F;
								break;
							case L_F_F:
								leftLegsBackward(LEFT_LEG_RANGE_DEGS, true);
								rightLegsBackward(RIGHT_LEG_RANGE_DEGS, false);
								state = State.L_B_B;
								break;
							default:
								break;
						}
						stepsTurned++;
					}
				} finally {
					stopped = true;
				}
			}
		};
		
		movementThread = new Thread(turnRightRunnable);
		movementThread.start();
	}
	
	private void returnToStartState() {
		stopped = false;
		
		try {
			switch (state) {
				case E_C_C:
					// nothing to do
					break;
				case L_C_C:
					this.rockRight(ROCKER_RANGE_DEGS / 2, false);
					break;
				case R_C_C:
					this.rockLeft(ROCKER_RANGE_DEGS / 2, false);
					break;
				case L_B_F:
					this.leftLegsForward(LEFT_LEG_RANGE_DEGS / 2, true);
					this.rightLegsBackward(RIGHT_LEG_RANGE_DEGS / 2, false);
					this.rockRight(ROCKER_RANGE_DEGS / 2, false);
					break;
				case R_F_B:
					this.leftLegsBackward(LEFT_LEG_RANGE_DEGS / 2, true);
					this.rightLegsForward(RIGHT_LEG_RANGE_DEGS / 2, false);
					this.rockLeft(ROCKER_RANGE_DEGS / 2, false);
					break;
				case L_F_B:
					this.leftLegsBackward(LEFT_LEG_RANGE_DEGS / 2, true);
					this.rightLegsForward(RIGHT_LEG_RANGE_DEGS / 2, false);
					this.rockRight(ROCKER_RANGE_DEGS / 2, false);
					break;
				case R_B_F:
					this.leftLegsForward(LEFT_LEG_RANGE_DEGS / 2, true);
					this.rightLegsBackward(RIGHT_LEG_RANGE_DEGS / 2, false);
					this.rockLeft(ROCKER_RANGE_DEGS / 2, false);
					break;
				case L_F_F:
					this.leftLegsBackward(LEFT_LEG_RANGE_DEGS / 2, true);
					this.rightLegsBackward(RIGHT_LEG_RANGE_DEGS / 2, false);
					this.rockRight(ROCKER_RANGE_DEGS / 2, false);
					break;
				case L_B_B:
					this.leftLegsForward(LEFT_LEG_RANGE_DEGS / 2, true);
					this.rightLegsForward(RIGHT_LEG_RANGE_DEGS / 2, false);
					this.rockRight(ROCKER_RANGE_DEGS / 2, false);
					break;
				case R_F_F:
					this.leftLegsBackward(LEFT_LEG_RANGE_DEGS / 2, true);
					this.rightLegsBackward(RIGHT_LEG_RANGE_DEGS / 2, false);
					this.rockLeft(ROCKER_RANGE_DEGS / 2, false);
					break;
				case R_B_B:
					this.leftLegsForward(LEFT_LEG_RANGE_DEGS / 2, true);
					this.rightLegsForward(RIGHT_LEG_RANGE_DEGS / 2, false);
					this.rockLeft(ROCKER_RANGE_DEGS / 2, false);
					break;
				default:
					break;
			}
			
			state = State.E_C_C;
		} finally {
			stopped = true;
		}
	}

	protected void leftLegsForward(int degrees, boolean immediateReturn) {
		leftMotor.rotate(-degrees, immediateReturn);
	}
	
	protected void leftLegsBackward(int degrees, boolean immediateReturn) {
		leftMotor.rotate(degrees, immediateReturn);
	}
	
	protected void rightLegsForward(int degrees, boolean immediateReturn) {
		rightMotor.rotate(-degrees, immediateReturn);
	}
	
	protected void rightLegsBackward(int degrees, boolean immediateReturn) {
		rightMotor.rotate(degrees, immediateReturn);
	}
	
	protected void rockLeft(int degrees, boolean immediateReturn) {
		centerMotor.rotate(degrees, immediateReturn);
	}
	
	protected void rockRight(int degrees, boolean immediateReturn) {
		centerMotor.rotate(-degrees, immediateReturn);		
	}
	
	private void moveForward(final int steps) {
		this.stop();
		
		this.stopped = false;
		
		Runnable forwardRunnable = new Runnable() {
			
			@Override
			public void run() {
				
				int stepsTaken = 0;
				
				try {
					while (!stopped && ( stepsTaken < steps || 0 == steps)) {
						switch (state) {
							case E_C_C:
								rockRight(ROCKER_RANGE_DEGS / 2, false);
								state = State.R_C_C;
								break;
							case R_C_C:
								leftLegsForward(LEFT_LEG_RANGE_DEGS / 2, true);
								rightLegsBackward(RIGHT_LEG_RANGE_DEGS / 2, false);
								state = State.R_F_B;
								break;
							case R_F_B:		
								rockLeft(ROCKER_RANGE_DEGS, false);
								state = State.L_F_B;
								break;
							case L_F_B:
								leftLegsBackward(LEFT_LEG_RANGE_DEGS, true);
								rightLegsForward(RIGHT_LEG_RANGE_DEGS, false);
								state = State.L_B_F;
								break;
							case L_B_F:
								rockRight(ROCKER_RANGE_DEGS, false);
								state = State.R_B_F;
								break;
							case R_B_F:
								leftLegsForward(LEFT_LEG_RANGE_DEGS, true);
								rightLegsBackward(RIGHT_LEG_RANGE_DEGS, false);
								state = State.R_F_B;
							default:
								break;
						}
						stepsTaken++;
					}
				} finally {
					stopped = true;
				}
			}
		};
		
		movementThread = new Thread(forwardRunnable);
		movementThread.start();
	}
	
	private void moveBackward(final int steps) {
		this.stop();
		
		this.stopped = false;
		
		Runnable backwardRunnable = new Runnable() {
			
			@Override
			public void run() {
				int stepsTaken = 0;
				
				try {
					while (!stopped && ( stepsTaken < steps || 0 == steps)) {
						switch (state) {
							case E_C_C:
								rockRight(ROCKER_RANGE_DEGS / 2, false);
								state = State.R_C_C;
								break;
							case R_C_C:
								leftLegsBackward(LEFT_LEG_RANGE_DEGS / 2, true);
								rightLegsForward(RIGHT_LEG_RANGE_DEGS / 2, false);
								state = State.R_B_F;
								break;
							case R_B_F:
								rockLeft(ROCKER_RANGE_DEGS, false);
								state = State.L_B_F;
								break;
							case L_B_F:
								leftLegsForward(LEFT_LEG_RANGE_DEGS, true);
								rightLegsBackward(RIGHT_LEG_RANGE_DEGS, false);
								state = State.L_F_B;
								break;
							case L_F_B:
								rockRight(ROCKER_RANGE_DEGS, false);
								state = State.R_F_B;
								break;
							case R_F_B:
								leftLegsBackward(LEFT_LEG_RANGE_DEGS, true);
								rightLegsForward(RIGHT_LEG_RANGE_DEGS, false);
								state = State.R_B_F;
								break;
							default:
								break;
						}
						stepsTaken++;
					}
				} finally {
					stopped = true;
				}
			}
		};
		
		movementThread = new Thread(backwardRunnable);
		movementThread.start();
	}
}
